/*
	This file is part of AVRCppLib.

    AVRCppLib is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    AVRCppLib is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with AVRCppLib.  If not, see <http://www.gnu.org/licenses/>.

    Copyright (C) Maxime Lemonnier 2007-2011
 */

#include "RCServo.h"


RCServo::RCServo(Comparator16 * comparator, IOAddress ocPinDDR, uint8_t ocPinNumber, uint16_t ocrValueFor1000us, uint16_t ocrValueFor2000us) :
comparator(comparator), ocrValueFor1000us(ocrValueFor1000us), ocrValueFor2000us(ocrValueFor2000us)
{

	setBit(*ocPinDDR, ocPinNumber);

	ticksPer_us = (float)ocrValueFor2000us / 2000.0;

}

RCServo::~RCServo()
{

}


void RCServo::setPulseWidth(uint16_t pulseWidth_us)
{

	uint16_t ocrValue = ticksPer_us * pulseWidth_us;

	if(ocrValue < ocrValueFor1000us)
		comparator->setOCRValue(ocrValueFor1000us);
	else if(ocrValue > ocrValueFor2000us)
		comparator->setOCRValue(ocrValueFor2000us);
	else
		comparator->setOCRValue(ocrValue);

}

uint16_t RCServo::getPulseWidth()
{
	return comparator->getOCRValue()/ticksPer_us;
}
